/*
 * Copyright (c) 2012 Andrea De Rinaldis <ozioso at ipignoli.dyndns.org>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

package org.dyndns.ipignoli.droidcruise.position;

import java.util.LinkedList;
import java.util.List;
import java.util.ListIterator;
import org.dyndns.ipignoli.droidcruise.routes.Route;


public class CurrentPosition{
  public static final int    MAX_LOCATION_KEEP_KM = 50;

  private static final int   STALE_SPEED_MSEC     = 2900;
  private static final int[] SPEED_REF            = {
      1, // 0 - 9
      1, // 10 - 19
      1, // 20 - 29
      1, // 30 - 39
      2, // 40 - 49
      2, // 50 - 59
      2, // 60 - 69
      3, // 70 - 79
      3, // 80 - 89
      4, // 90 - 99
      5, // 100 - 109
      6
                                                  };

  private double[]           speedRef;

  private List<MyLocation>   locations;

  public CurrentPosition(){
    locations = new LinkedList<MyLocation>();
    speedRef = new double[SPEED_REF[SPEED_REF.length - 1]];
  }

  public synchronized boolean setPosition(MyLocation l){
    return locations.add(l);
  }

  public synchronized MyLocation getLastPosition(){
    if(locations.size() == 0)
      return null;
    return locations.get(locations.size() - 1);
  }

  public synchronized void updateLastPosition(){
    if(locations.size() == 0)
      return;
    MyLocation lastInserted = locations.get(locations.size() - 1);
    if(lastInserted.getTime() < System.currentTimeMillis() - STALE_SPEED_MSEC)
      setPosition(lastInserted.duplicate());
  }

  public synchronized double getCurrentSpeed(){
    if(locations.size() <= 1)
      return 0;

    MyLocation last = locations.get(locations.size() - 1);
    MyLocation previous = locations.get(locations.size() - 2);
    double distance = previous.distanceTo(last);

    if(distance == 0d)
      addSpeedRef(0d);
    else if(last.getSpeed() == 0f)
      addSpeedRef(distance * 3600 / (last.getTime() - previous.getTime()));
    else
      addSpeedRef(last.getSpeed() * 3600 / 1000);

    return getMedian(speedRef);
  }

  public synchronized double getAverageSpeed(){
    if(locations.size() <= 1)
      return 0d;

    double distanceTot = 0d, distanceSpeed = 0d;
    MyLocation previousTot = null, previousSpeed = null, curr = null;
    Route lastRoute = locations.get(locations.size() - 1).getRoute();
    for(ListIterator<MyLocation> it = locations.listIterator(locations.size()); it
        .hasPrevious();){
      curr = it.previous();

      if(previousTot != null)
        distanceTot += previousTot.distanceTo(curr);
      previousTot = curr;
      if(distanceTot > MAX_LOCATION_KEEP_KM * 1000 && lastRoute == null){
        it.remove();
        continue;
      }

      if(lastRoute == null || lastRoute.equals(curr.getRoute())){
        if(previousSpeed != null)
          distanceSpeed += previousSpeed.distanceTo(curr);
        previousSpeed = curr;
      }
    }

    long t =
        previousSpeed == null ? 0 : locations.get(locations.size() - 1)
            .getTime() - previousSpeed.getTime();

    if(t == 0l)
      return 0d;

    return distanceSpeed * 3600 / t;
  }

  private void addSpeedRef(double speed){
    for(int i = 0; i < speedRef.length - 1; i++)
      speedRef[i] = speedRef[i + 1];
    speedRef[speedRef.length - 1] = speed;
  }

  private double getMedian(double[] speedRef){
    double speed = 0d;
    int slots =
        SPEED_REF[Math.min((int)speedRef[speedRef.length - 1] / 10,
            SPEED_REF.length - 1)];
    double tot = 0d, weight;
    for(int i = 0; i < slots; i++){
      weight = 1 + (slots == 1 ? 0 : (i / (slots - 1)));
      speed += speedRef[speedRef.length - 1 - i] * weight;
      tot += weight;
    }

    return speed / tot;

    // double speed = 0;
    // int tot;
    // for(tot = 0; tot < SPEED_REF[Math.min(
    // (int)speedRef[speedRef.length - 1] / 10, SPEED_REF.length - 1)]; tot++)
    // speed += speedRef[speedRef.length - 1 - tot];
    // return speed / tot;

    // double[] speedRefTemp = speedRef.clone();
    // Arrays.sort(speedRefTemp);
    // if(speedRefTemp.length / 2 * 2 == speedRefTemp.length)
    // return (speedRefTemp[speedRefTemp.length / 2 - 1] +
    // speedRefTemp[speedRefTemp.length / 2]) / 2;
    // return speedRefTemp[speedRefTemp.length / 2];
  }
}
